#pragma once

#include "vio/front_end/feature.h"
#include "vio/front_end/frame.h"
#include "vio/vio_common.h"

namespace vio {
class Frame;
struct Feature;

typedef Eigen::Matrix<double, 2, 3> Matrix23d;

// a 3D point on the surface of the scene
class Point : private boost::noncopyable {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  typedef std::shared_ptr<Point> Ptr;

  enum PointType { TYPE_DELETED, TYPE_CANDIDATE, TYPE_UNKNOWNM, TYPE_GOOD };

 public:
  static int point_counter_;  // counts the number of created points, used to
                              // set the unique id.
  int id_;                    // unique ID of the point
  Eigen::Vector3d pos_;       // 2d pos of the point in the world frame
  Eigen::Vector3d normal_;    // surface normal at point
  Eigen::Matrix3d
      normal_information_;  // inverse covariance matrix of normal estimation
  bool normal_set_;  // flag whether the surface normal was estimated or not.
  std::list<std::shared_ptr<Feature>>
      obs_;                // reference to keyframes which observe the point
  size_t n_obs_;           // number of observations: keyframes AND successful
                           // reprojections in intermediate frames.
  int last_published_ts_;  // timestamp of last publishing
  int last_projected_kf_id_;  // flag for the reprojection: don't reproject a pt
                              // twice
  PointType type_;            // quality of the point
  int n_failed_reproj_;  // number of failed reprojections. used to asses the
  // quality of the point
  int n_succeeded_reproj_;  // number of succeeded reprojections. used to assess
                            // the quality of the point.
  int last_structure_optim_ts_;  // timestamp of the last point optimization.

  Point(const Eigen::Vector3d& pos);
  Point(const Eigen::Vector3d& pos, std::shared_ptr<Feature> ftr);

  // add a reference to a frame
  void AddFrameRef(std::shared_ptr<Feature> ftr);

  // remove reference to a frame
  bool DeleteFrameRef(std::shared_ptr<Frame> frame);

  // initialize point normal, the initial estimate will point toward the frame
  void InitNormal();

  // check whether mappoint has reference to a frame
  std::shared_ptr<Feature> FindFrameRef(std::shared_ptr<Frame> frame);

  // // get frame with similar viewpoint
  bool GetCloseViewObs(const Eigen::Vector3d& pos,
                       std::shared_ptr<Feature>& ftr) const;

  // get number of observations
  inline size_t NRefs() const {
    return obs_.size();
  }

  // optimize point position through minimizing the reprojection error
  void Optimize(const size_t n_iter);

  // jacobian of point projection on unit plane(focal length = 1) in frame(f)
  inline static void jacobian_xyz2uv(const Eigen::Vector3d& xyz_cam,
                                     const Eigen::Matrix3d& R_f_w,
                                     Matrix23d& point_jac) {
    const double z_inv = 1.0 / xyz_cam[2];
    const double z_inv_2 = z_inv * z_inv;

    point_jac(0, 0) = z_inv;
    point_jac(0, 1) = 0.0;
    point_jac(0, 2) = -xyz_cam[0] * z_inv_2;

    point_jac(1, 0) = 0.0;
    point_jac(1, 1) = z_inv;
    point_jac(1, 2) = -xyz_cam[1] * z_inv_2;

    point_jac = -point_jac * R_f_w;
  }
};
}  // namespace vio
